#include <vtkSmartPointer.h>
#include <vtkPolyData.h>
#include <vtkModifiedBSPTree.h>
#include <vtkSphereSource.h>
#include <vtkPolyDataReader.h>
#include "vtkDoubleArray.h"
#include <vtkPointData.h>
#include <vtkMath.h>
#include "vtkPolyDataWriter.h"

// HYL 10/25/13
#include <vtkVersion.h>
#include <vtkCenterOfMass.h>
#include <vtkDoubleArray.h>
#include <vtkPoints.h>
#include <vtkPolyData.h>
#include <vtkSmartPointer.h>

#include <limits>
#include <cmath>
#include <time.h>
#include <stdio.h>
#include <iostream>
#include <fstream>
#include <string>
#include <string.h>


/*
./MotionAnalysis convexHull_SPHARM1.vtk convexHull_SPHARM2.vtk convexHull_SPHARM3.vtk distance.txt
./Bullseye distance.txt distanceBullseye.txt
*/

int main(int argc, char * argv[])
{
	int x1Position = 0, y1Position = 0, x2Position = 0, y2Position = 0, x3Position = 0, y3Position = 0;
	bool refBasalPoint1=false, refBasalPoint3=false;
	float inSliceSpacing = 0;


	std::string bullseyeFile; // output regional curvature in bull's-eye format
//  Load phase1
	vtkPolyDataReader *Reader1 = vtkPolyDataReader::New();
	Reader1->SetFileName(argv[1]);
	Reader1->Update();
	vtkSmartPointer < vtkPolyData > PolyData1 = Reader1->GetOutput();

// 	Create the bspTree for phase1
	vtkSmartPointer<vtkModifiedBSPTree> Tree1 = vtkSmartPointer<vtkModifiedBSPTree>::New();
	Tree1->SetDataSet(Reader1->GetOutput());
	Tree1->BuildLocator();

////	Load phase2
//	vtkPolyDataReader *Reader2 = vtkPolyDataReader::New();
//	Reader2->SetFileName(argv[2]);
//	Reader2->Update();
//	vtkSmartPointer < vtkPolyData > PolyData2 = Reader2->GetOutput();
//
//// 	Create the bspTree for phase2
//	vtkSmartPointer<vtkModifiedBSPTree> Tree2 = vtkSmartPointer<vtkModifiedBSPTree>::New();
//	Tree2->SetDataSet(Reader2->GetOutput());
//	Tree2->BuildLocator();
//
////	Load phase3
//	vtkPolyDataReader *Reader3 = vtkPolyDataReader::New();
//	Reader3->SetFileName(argv[3]);
//	Reader3->Update();
//	vtkSmartPointer < vtkPolyData > PolyData3 = Reader3->GetOutput();
//
//// 	Create the bspTree for phase3
//	vtkSmartPointer<vtkModifiedBSPTree> Tree3 = vtkSmartPointer<vtkModifiedBSPTree>::New();
//	Tree3->SetDataSet(Reader3->GetOutput());
//	Tree3->BuildLocator();


//	Compute the center of mass1
	vtkSmartPointer < vtkCenterOfMass > centerOfMassFilter1 = vtkSmartPointer< vtkCenterOfMass > ::New();
	centerOfMassFilter1->SetInput(Reader1->GetOutput() );
	centerOfMassFilter1->SetUseScalarsAsWeights(false);
	centerOfMassFilter1->Update();

	double center1[3];
	centerOfMassFilter1->GetCenter(center1);
//	std::cout << "Mass center1 is: " << center1[0] << " " << center1[1]<< " " << center1[2] << std::endl;

////	Compute the center of mass2
//	vtkSmartPointer < vtkCenterOfMass > centerOfMassFilter2 = vtkSmartPointer< vtkCenterOfMass > ::New();
//	centerOfMassFilter2->SetInput(Reader2->GetOutput() );
//	centerOfMassFilter2->SetUseScalarsAsWeights(false);
//	centerOfMassFilter2->Update();
//
//	double center2[3];
//	centerOfMassFilter2->GetCenter(center2);
//	std::cout << "Mass center2 is: " << center2[0] << " " << center2[1]<< " " << center2[2] << std::endl;
//
////	Compute the center of mass3
//	vtkSmartPointer < vtkCenterOfMass > centerOfMassFilter3 = vtkSmartPointer< vtkCenterOfMass > ::New();
//	centerOfMassFilter3->SetInput(Reader3->GetOutput() );
//	centerOfMassFilter3->SetUseScalarsAsWeights(false);
//	centerOfMassFilter3->Update();
//
//	double center3[3];
//	centerOfMassFilter3->GetCenter(center3);
//	std::cout << "Mass center3 is: " << center3[0] << " " << center3[1]<< " " << center3[2] << std::endl;

  	int n_vert = PolyData1->GetNumberOfPoints();
  	double distance[n_vert];

// Write all of the coordinates of the points in the vtkPolyData to the console.
  	for(vtkIdType i = 0; i < n_vert; i++)
    {
	  double p[3];
	  PolyData1->GetPoint(i,p); // This is identical to: PolyData1->GetPoints()->GetPoint(i,p);

	  // Find the squared distance between the points.
	  double squaredDistance = vtkMath::Distance2BetweenPoints(p, center1);

	  // Take the square root to get the Euclidean distance between the points.
	  double d = sqrt(squaredDistance);
	  distance[i] = d;
//	  std::cout << "Point " << i << " : (" << p[0] << " " << p[1] << " " << p[2] << ")" << ", " << "d = " << d << std::endl;
//	  std::cout << "Point" << "[" << i << "] " << "Distance " << " : " << distance[i] << std::endl;
    }
//  std:cout << " Distance " << distance[0] << distance[1] << ", " << distance[2] << ", " << distance[3] << ", " << distance[4] << std::endl;

  	std::string  outputFilename;
  	outputFilename = argv[2];
    std::ofstream myfile(outputFilename.c_str(), std::ios::out);
    myfile.precision(10);
    for (int i=0; i<n_vert; i++)
    {
        myfile << "distance["<<i<<"]: " << distance[i] << std::endl;
    }
    myfile.close();
  return EXIT_SUCCESS;
}
